坐标系标定算法及应用说明

[TOC]

0. 概述

机器人坐标系标定可以概括为以下两类：

• 用户坐标系标定: 获得用户坐标系在基坐标系的描述
• 工具坐标系标定: 获得工具坐标系在法兰坐标系的描述

下面以末端工具为例说明用户坐标系和工具坐标系的标定方法

1. 用户坐标系标定

用户坐标系｛U｝即用户自定义坐标系；机器人可以和不同的工作台或夹具配合工作， 在每个工作台上建立一个用户坐标系。通过该标定方法可以得到任意用户坐标系在基坐标系下的描述 ： $^{B}T_{U}$ 。

标定步骤

通常我们采用三点标定法来对坐标系进行标定，

标定步骤为：

1. 第一个点为用户坐标系原点；
2. 第二个点为 X, Y, Z 轴方向向量上的一个点；
3. 第三个点为剩余两轴平面上的点。

下面以XOXY 为例说明标定步骤和算法实现；

算法实现

采用 XOXY 方式标定算法实现方式为，$z = x \times y$ , $y = z \times x$ ;

该方法标定依赖 机器人法兰位姿精度 和 工具坐标系的位置精度。 ${}^{b}P_t ={}^bR_f *{}^fP_t + {}^bP_f$

接口调用

    /**
     * @brief 标定用户坐标系的位置和姿态在基坐标系的描述
     * @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
     * @param F_b_t_calib_point: 标定点(tcp)在基坐标系下的位姿( = 3), 其顺序和标定类型的描述一致
     * 下面以标定类型为 O_X_XY 为例说明参数的含义:
     * 1) 运动到用户坐标系原点，记录此时 tcp 在基坐标下的位姿，为 F_b_t_calib_point[0] 的输入;
     * 2) 沿用户坐标系X轴运动一定的距离，记录此时 tcp 在基坐标下的位姿，为 F_b_t_calib_point[1] 的输入;
     * 3) 沿用户坐标系XY平面运动一定的距离，记录此时 tcp 在基坐标下的位姿，为 F_b_t_calib_point[2] 的输入;
     * @param F_b_u: 用户坐标系位姿在基坐标系的描述
     * @return: 返回值 < 0, 表示标定失败
     */
    ARAL_API_BASIC(1.0) int calibUserFrameInBase(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_t_calib_point, interface::RLPose& F_b_u) = 0;

2. 工具坐标系标定

工业机器人使用过程中经常在机器人末端法兰面安装不同的工具来满足实际生产需求，为了准确控制工具运动的位置与姿态，需要对工具所在坐标系进行标定。

工具标定的目标是获得工具相对于法兰坐标系的描述： ${}^{F}T_{T}$ 。

2.1 坐标系原点标定

标定步骤

通常我们采用四点标定法来对工具的位置进行标定。

步骤为：保持末端工具位置不变，改变机器人关节角，得到四个及以上的标定构型。

算法实现

工具在基坐标系的位置可以表示为： ${}^bP_t ={}^bR_f *{}^fP_t + {}^bP_f$ 由运动学正解可以得到任两个标定点的方程为： ${}^bP_{t} ={}^bR_{f1} *{}^fP_t + {}^bP_{f1} {}^bP_{t} ={}^bR_{f2} *{}^fP_t + {}^bP_{f2}$ 标定点保持末端工具位置不变，$^bP_t$ 保持不变；联立方程可以得到方程： $0 =({}^bR_{f1} - {}^bR_{f2}) * {}^fP_t + ({}^bP_{f1} - {}^bP_{f2})$ 采用 n 个点进行位置标定可以得到 n-1 个方程；采用最小二乘法对参数进行估计可以得到 ${}^fP_t$ 。

1. 采用两点标定，可以得到一个方程，辨识矩阵不满秩，$|R_1 - R_2| = |R_1| - |R_2| = 0$；
2. 采用 >= 三点标定，可以得到两个方程；通常可以求解。
3. 通常情况采用四个点进行标定，确保辨识矩阵满秩，减小标定误差。

接口调用

    /**
     * @brief 标定工具坐标系的位置在法兰坐标系的描述
     * @param F_b_f_calib_point: 法兰在基坐标下的位姿( >= 4), 保持末端 TCP 不动，让机械臂运动到四个及以上的构型，记录此时法兰在基坐标下的位姿，即为此输入.
     * @param P_f_t: 工具坐标系的位置在法兰坐标系的描述
     * @param error: 标定后最大的位置误差
     * @return: 返回值 < 0, 表示标定失败
     */
    ARAL_API_BASIC(1.0) int calibToolPosInFlange(const std::vector<RLPose>& F_b_f_calib_point, interface::Vector3d& P_f_t, double& error) = 0;

2.2 坐标系方向标定

标定步骤

工具坐标系的姿态标定步骤为：

保持机械臂姿态不变，

1. 第一个点为工具坐标系原点；
2. 第二个点为 X, Y, Z 轴方向向量上的一个点；
3. 第三个点为剩余两轴方向向量上的一个点，或是平面上的点。

标定工具坐标系姿态和用户坐标系姿态方法一样，但是需要保持机械臂姿态不变

算法实现

任意两点可以得到如下方程： ${}^bP_{t1} = {}^bR_{f} *{}^fP_t + {}^bP_{f1} {}^bP_{t2} = {}^bR_{f} *{}^fP_t + {}^bP_{f2} {}^bv12 = {}^bP_{t1} - {}bP_{t2}$ 向量 ${}^bv{12}$ 的方向即为工具坐标系的方向向量在基坐标系的表示；由于姿态保持不变，可以得到此方向向量在法兰坐标系的表示，为： ${}^fv_{12} =({}^bR_{f})^{-1} * {}^bv_{12}$ 从而得到工具坐标系在法兰坐标系中的位姿 ${}^fR{t}$

那么工具坐标系为：${}^fT{t} = [{}^fR{t} ,{}^fP_{t}]$

接口调用

    /**
     * @brief 标定工具坐标系的姿态在法兰坐标系的描述
     * @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
     * @param F_b_f_calib_point: 法兰在基坐标系的位姿( = 3), 其顺序和标定类型的描述一致(保持法兰姿态不变)
     * 下面以标定类型为 O_X_XY 为例说明参数的含义:
     * 1) 运动到工具坐标系原点，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[0] 的输入;
     * 2) 保持末端姿态不动，沿工具坐标系X轴运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[1] 的输入;
     * 3) 保持末端姿态不动，沿工具坐标系XY平面运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[2] 的输入;
     * @param R_f_t: 工具坐标系的姿态在法兰坐标系的描述
     * @return: 返回值 < 0, 表示标定失败
     */
    ARAL_API_BASIC(1.0) int calibToolOriInFlange(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_f_calib_point, interface::Vector3d& R_f_t) = 0;

3 参考文献

• 熊烁, 叶伯生, 蒋明. 机器人工具坐标系标定算法研究[J]. 机械与电子, 2012, 000(006):60-63.
• 工业机器人工具坐标系（TCF）标定的六点法原理